#include "cartographer/rcm/imu/pose_integrate.h"
#include "cartographer/rcm/imu/filter.h"

namespace cartographer
{
    /*
    如果是5个IMU数据，那么计算对是[0,1],[1,2],[2,3],[3,4] 则preIdxMax=3,[0,1]和[3,4]会进行补偿;因为第1和第5个IMU数据的
    时间戳不一定跟位姿起始和结束时间完全对齐;
    */
    int PoseIntegrator::Integrate(const ImuMeasSet &imuSet, const common::Time &timeStart, const common::Time &timeEnd)
    {
        if (timeEnd <= timeStart)
            return -1;
        timeEnd_ = timeEnd;
        timeStart_ = timeStart;
        int count = 0;
        double tstep = 0;
        Eigen::Vector3d acc;
        Eigen::Vector3d angular;
        size_t preIdxMax = imuSet.size() - 2;
        for (size_t i = 0; i <= preIdxMax; i++)
        {
            auto curr = imuSet[i];
            auto next = imuSet[i + 1];
            if (filter_ != nullptr)
                next = filter_->Estimate(curr, next);
            // LOG(INFO) << "\nImuMiddle[" << i << "," << i + 1 << "]\n"
            //           << "curr:\n"
            //           << "acc:" << VecToStr(curr.acceleration) << "\n"
            //           << "ang:" << VecToStr(curr.angular) << "\n"
            //           << "time:" << common::ToUniversal(curr.time) << "\n"
            //           << "next:\n"
            //           << "acc:" << VecToStr(next.acceleration) << "\n"
            //           << "ang:" << VecToStr(next.angular) << "\n"
            //           << "time:" << common::ToUniversal(next.time) << "\n";
            if (HasNan(next.acceleration) || HasNan(next.angular))
            {
                LOG(INFO) << "ImuHasNan";
                continue;
            }
            if (i == 0 && i < preIdxMax)
            {
                ImuCompensateToStartTime(curr, next, timeStart_);
                acc = 0.5 * (curr.acceleration + next.acceleration);
                angular = 0.5 * (curr.angular + next.angular);
                tstep = common::ToSeconds(next.time - timeStart_);
            }
            else if (i < preIdxMax)
            {
                // 中间的IMU数据直接求平均
                acc = (curr.acceleration + next.acceleration) * 0.5f;
                angular = (curr.angular + next.angular) * 0.5f;
                tstep = common::ToSeconds(next.time - curr.time);
            }
            else if (i > 0 && i == preIdxMax)
            {
                ImuCompensateToEndTime(curr, next, timeEnd_);
                acc = 0.5 * (curr.acceleration + next.acceleration);
                angular = 0.5 * (curr.angular + next.angular);
                tstep = common::ToSeconds(timeEnd_ - curr.time);
            }
            else if (i == 0 && i == preIdxMax)
            {
                // 只有两个IMU数据，不做补偿
                acc = curr.acceleration;
                angular = curr.angular;
                tstep = common::ToSeconds(timeEnd_ - timeStart_);
            }
            // LOG(INFO) << "\nMidImuRes: "
            //           << "acc:" << VecToStr(acc) << " "
            //           << "ang:" << VecToStr(angular) << " "
            //           << "time:" << tstep << "\n";
            count++;
            integrator_->Update(acc, angular, tstep);
        }
        if (count > 0)
            integrator_->hasIntegrated = true;
        return count;
    }

    PVR PoseIntegrator::Predict(const PVR &pvrLast, const ImuBias &biasNew)
    {
        assert(common::ToUniversal(timeStart_) == common::ToUniversal(pvrLast.time));
        auto duration = common::ToSeconds(timeEnd_ - timeStart_);
        if (duration < integrator_->sumTime_)
            duration = integrator_->sumTime_;
        // 在预积分后偏置可能会被优化(即产生新的偏置)，函数SetNewBias内会计算与积分时的偏置的变化量
        // 然后调用GetUpdatedDeltaRotation等函数时会利用偏置的变化量乘以偏置的雅可比得到新的PVQ增量
        // 如果这个bias_new与bias_init_一样,则PVQ增量就是预积分时计算的结果
        integrator_->SetNewBias(biasNew);
        // PVR增量
        PVR incre = integrator_->GetUpdatedDeltaPVR();
        // PrintPVR("incre", incre);
        //  当前PVR
        PVR pvrCurr = AddPVR(pvrLast, incre, GRAVITY_NEGATIVE, duration);
        pvrCurr.time = timeEnd_;
        return pvrCurr;
    }

    void PoseIntegrator::UpdateFilter(const common::Time &time)
    {
        if (filter_ == nullptr)
            return;
        double freeProb = MatchSupervisor::GetInstance().residualProb_;
        FilterFactor currFc;
        currFc.time = time;
        currFc.averageOccupyProb = 1.0 - freeProb;
        if (filterFactors_.size() < 2)
        {
            filterFactors_.push(currFc);
            return;
        }
        const auto &lastFc = filterFactors_.back();
        if (lastFc.averageOccupyProb > currFc.averageOccupyProb)
        {
            // 概率在减小，说明噪声越大，阈值应该越小
            currFc.threshAcc = filter_->threshAcc_;
            currFc.threshGyro = filter_->threshGyro_;
        }
        else
        {
            currFc.threshAcc = filter_->threshAcc_;
            currFc.threshGyro = filter_->threshGyro_;
        }
        filterFactors_.push(currFc);
    }
} // namespace cartographer